#include <chrono>
#include <memory>
#include <iostream>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "yaogan.hpp"
#include "motor_drv.hpp"
using namespace std::chrono_literals;

/* This example creates a subclass of Node and uses std::bind() to register a
 * member function as a callback from the timer. */

class MinimalPublisher : public rclcpp::Node
{
public:
  MinimalPublisher()
  : Node("minimal_publisher"), count_(0)
  {
    publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
    timer_ = this->create_wall_timer(
      50ms, std::bind(&MinimalPublisher::timer_callback, this));
       yg=new yaogan("ttyS6",115200); 
        mc = new Motor_drv("ttyS4",1000000);
  }

private:
  void timer_callback()
  {
    auto message = std_msgs::msg::String();
    message.data = "Hello, world! " + std::to_string(count_++);
    int speed=yg->yg_value.Left_vertic*8;
    int angle=yg->yg_value.Right_horiz*(-16384)/103;
    // RCLCPP_INFO(this->get_logger(), "Publishing: '%s' yao=%d ag=%d", message.data.c_str(),speed,angle);
    // RCLCPP_INFO(this->get_logger(), "current angle=%d",mc->angle_cur);
    // int speed=yg->yg_value.Left_vertic*8;
    // int angle=yg->yg_value.Right_horiz*(-16384)/103;
    mc->set_speed(speed,angle);
    publisher_->publish(message);
  }
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
  size_t count_;
  yaogan *yg;
  Motor_drv *mc;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalPublisher>());
  rclcpp::shutdown();
  return 0;

}
